#define _CRT_SECURE_NO_WARNINGS
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <boost\asio.hpp>
#include <boost\algorithm\string.hpp>
#include <Message.h>
#include <iostream>

using namespace std;
using boost::asio::ip::tcp;


extern "C" {

#include "extApi.h"
	/*	#include "extApiCustom.h" if you wanna use custom remote API functions! */
}


void getmemory(unsigned char **p, int rX, int rY){
	*p = (unsigned char *)malloc(rX*rY * 3 * sizeof(unsigned char));
}

void printInSequence(unsigned char *bufferImage, int resolutionX, int resolutionY)  //print in sequence from 1 to X*Y*3
{
	int Kprint;
	int Kcount;
	Kcount = resolutionX*resolutionY * 3;
	for (Kprint = 0; Kprint != Kcount; Kprint++)
	{
		printf("unsigned bufferImage[%d]=%d\n", Kprint, (unsigned char)bufferImage[Kprint]); // convert to unsigned char and output
		printf("bufferImage[%d]=%d\n", Kprint, bufferImage[Kprint]);
	}
}


//----------------------------------------------


int __counter = 0;

int main(int argc, char* argv[])
{

	string programName = "semaforo";


	int portNb = 0;
	int redundantTipHandle;
	int rotationHandle;

	if (argc >= 4)
	{
		portNb = atoi(argv[1]);
		redundantTipHandle = atoi(argv[2]);
		rotationHandle = atoi(argv[3]);
	}
	else
	{
		printf("Indicate following arguments: 'portNumber redundantTipHandle'!\n");
		extApi_sleepMs(5000);
		return 0;
	}

	bool connected = false;

	//try connect to message server
	boost::asio::io_service ioService;

	tcp::resolver resolver(ioService);
	tcp::resolver::query query(tcp::v4(), "localhost", "1313");
	tcp::resolver::iterator iterator = resolver.resolve(query);

	tcp::socket s(ioService);
	boost::asio::connect(s, iterator);
	connected = true;

	
	int clientID = simxStart((simxChar*)"127.0.0.1", portNb, true, true, 2000, 5);
	if (clientID != -1)
	{
		int cont = 0;
		int semaphorePosition = 0;
		simxUChar redundantTipPosition = 0;

		//message to verify is there is any messages for me
		string msgAnyMessage = programName + " server";
		MessageHeader header(msgAnyMessage.length() + 5, MessageType::ANY_MESSAGE_FOR_ME);
		Message anyMessage(header, msgAnyMessage);
		string msgAnyMessageBuffer = anyMessage.GetMessageBuffer();


		while (simxGetConnectionId(clientID) != -1)
		{


			if (cont >= 500)
			{
				cont = 0;
				if (semaphorePosition == 1)
					semaphorePosition = 0;
				else
					semaphorePosition = 1;
			}
			simxSetJointTargetVelocity(clientID, redundantTipHandle, 0.1, simx_opmode_oneshot);
			simxSetJointTargetVelocity(clientID, rotationHandle, 0.1, simx_opmode_oneshot);

			if (semaphorePosition == 0)
			{
				simxSetJointTargetPosition(clientID, redundantTipHandle, 3.1415 * 30.0 / 180.0, simx_opmode_oneshot);
				simxSetJointTargetPosition(clientID, rotationHandle, 3.1415 * 5.0 / 180.0, simx_opmode_oneshot);
			}
			else
			{
				simxSetJointTargetPosition(clientID, redundantTipHandle, -3.1415 * 5.0 / 180.0, simx_opmode_oneshot);
				simxSetJointTargetPosition(clientID, rotationHandle, -3.1415 / 4.0, simx_opmode_oneshot);
			}

			cont++;


			//send request to server

			if (__counter % 50 == 0)
			{
				__counter = 0;
				boost::asio::write(s, boost::asio::buffer(msgAnyMessageBuffer.c_str(), msgAnyMessageBuffer.length()));

				vector<char> replyHeader(5);
				boost::asio::read(s, boost::asio::buffer(replyHeader));

				int messageLength = 0;
				messageLength |= replyHeader[0] << 24;
				messageLength |= replyHeader[1] << 16;
				messageLength |= replyHeader[2] << 8;
				messageLength |= replyHeader[3];

				int msgType = replyHeader[4];

				if (msgType == MessageType::REQUEST_INFO)
				{

					vector<char> replyMessage(messageLength - replyHeader.size());
					boost::asio::read(s, boost::asio::buffer(replyMessage));

					string strMsg(replyMessage.data(), replyMessage.size());
					cout << strMsg << endl;

					vector<string> subStrings;
					boost::split(subStrings, strMsg, boost::is_any_of(" "));

					string source = subStrings[0];
					
					string varName = subStrings[2];

					
					string strAnswer = programName + " ";
				
					strAnswer = strAnswer.append(source);
					
					cout << strAnswer << endl;

					if (varName == "semaphorePosition")
					{

						strAnswer = strAnswer.append(" semaphorePosition ");
						std::ostringstream stream;
						stream << semaphorePosition;
						strAnswer = strAnswer.append(stream.str());

						int messageLength = 5 + strAnswer.size();
						MessageHeader ansHeader(messageLength, MessageType::SEND_INFO);
						Message asnMsg(ansHeader, strAnswer);
						string ansStr = asnMsg.GetMessageBuffer();

						boost::asio::write(s, boost::asio::buffer(ansStr.c_str(), ansStr.length()));

					}
				}
			}

			__counter++;
			extApi_sleepMs(5);
		}
		simxFinish(clientID);
	}
	return(0);
}

